
int t;

IMyShipController Cockpit;

// 车轮
Wheel L1;
Wheel L2; 
Wheel L3;
Wheel R1;
Wheel R2;
Wheel R3;

void Main(){
    // Wheel = GridTerminalSystem.GetBlockWithName("Wheel") as IMyMotorSuspension;
    if(L1 == null){init(); return;}

    t ++;

    MotionInit(); //运动初始化

    Move((double)-Cockpit.MoveIndicator.Z); //根据WS前进后退

    Rise(Cockpit.MoveIndicator.Y); //根据空格C起降

    Turn(Cockpit.MoveIndicator.X*60); //根据AD转向

    checkBZ(); //检测避震

    IMyTextPanel LCD = GridTerminalSystem.GetBlockWithName("LCD") as IMyTextPanel;
	if(LCD != null){
		LCD.ShowPublicTextOnScreen();
		string str = L1.isOnGround().ToString();
		LCD.WritePublicText(str);
	}
}

//初始化获取方块
void init(){
    Cockpit = GridTerminalSystem.GetBlockWithName("Cockpit") as IMyShipController;
    L1 = new Wheel(
        getGroupBlocksByBlockName("[BZG]L1", "[BZ]Wheel") as IMyMotorSuspension,
        getGroupBlocksByBlockName("[BZG]L1", "[BZ]Rotor") as IMyMotorStator,
        getGroupBlocksByBlockName("[BZG]L1", "[BZ]Piston") as IMyPistonBase,
        getGroupBlocksByBlockName("[BZG]L1", "[BZ]Camera") as IMyCameraBlock
    );
    L2 = new Wheel(
        getGroupBlocksByBlockName("[BZG]L2", "[BZ]Wheel") as IMyMotorSuspension,
        getGroupBlocksByBlockName("[BZG]L2", "[BZ]Rotor") as IMyMotorStator,
        getGroupBlocksByBlockName("[BZG]L2", "[BZ]Piston") as IMyPistonBase,
        getGroupBlocksByBlockName("[BZG]L2", "[BZ]Camera") as IMyCameraBlock
    );
    L3 = new Wheel(
        getGroupBlocksByBlockName("[BZG]L3", "[BZ]Wheel") as IMyMotorSuspension,
        getGroupBlocksByBlockName("[BZG]L3", "[BZ]Rotor") as IMyMotorStator,
        getGroupBlocksByBlockName("[BZG]L3", "[BZ]Piston") as IMyPistonBase,
        getGroupBlocksByBlockName("[BZG]L3", "[BZ]Camera") as IMyCameraBlock
    );
    R1 = new Wheel(
        getGroupBlocksByBlockName("[BZG]R1", "[BZ]Wheel") as IMyMotorSuspension,
        getGroupBlocksByBlockName("[BZG]R1", "[BZ]Rotor") as IMyMotorStator,
        getGroupBlocksByBlockName("[BZG]R1", "[BZ]Piston") as IMyPistonBase,
        getGroupBlocksByBlockName("[BZG]R1", "[BZ]Camera") as IMyCameraBlock
    );
    R2 = new Wheel(
        getGroupBlocksByBlockName("[BZG]R2", "[BZ]Wheel") as IMyMotorSuspension,
        getGroupBlocksByBlockName("[BZG]R2", "[BZ]Rotor") as IMyMotorStator,
        getGroupBlocksByBlockName("[BZG]R2", "[BZ]Piston") as IMyPistonBase,
        getGroupBlocksByBlockName("[BZG]R2", "[BZ]Camera") as IMyCameraBlock
    );
    R3 = new Wheel(
        getGroupBlocksByBlockName("[BZG]R3", "[BZ]Wheel") as IMyMotorSuspension,
        getGroupBlocksByBlockName("[BZG]R3", "[BZ]Rotor") as IMyMotorStator,
        getGroupBlocksByBlockName("[BZG]R3", "[BZ]Piston") as IMyPistonBase,
        getGroupBlocksByBlockName("[BZG]R3", "[BZ]Camera") as IMyCameraBlock
    );

    R1.WheelLeftRight = -1; R2.WheelLeftRight = -1; R3.WheelLeftRight = -1;
}

//动作归位，活塞收缩，角度归零
void MotionInit(){
    if(L1.Rise(0)){
        L1.RotorLock();
    }
    if(L2.Rise(0)){
        L2.RotorLock();
    }
    if(L3.Rise(0)){
        L3.RotorLock();
    }
    if(R1.Rise(0)){
        R1.RotorLock();
    }
    if(R2.Rise(0)){
        R2.RotorLock();
    }
    if(R3.Rise(0)){
        R3.RotorLock();
    }
}

//前进后退
void Move(double velocity = 0){
    L1.Move(velocity); L2.Move(velocity); L3.Move(velocity); 
    R1.Move(velocity); R2.Move(velocity); R3.Move(velocity); 
}

//升降
bool Rise(double x = 2, double velocity = 0){
    bool a = L1.Rise(x, velocity);
    bool b = L2.Rise(x, velocity);
    bool c = L3.Rise(x, velocity);
    bool d = R1.Rise(x, velocity);
    bool e = R2.Rise(x, velocity);
    bool f =  R3.Rise(x, velocity);
    return a && b && c && d && e && f;
}

//转向
bool Turn(double x = 0, double velocity = 0){
    bool a = L1.Turn(new Angle(-x), velocity);
    bool b = L2.Turn(new Angle(), velocity);
    bool c = L3.Turn(new Angle(x), velocity);
    bool d = R1.Turn(new Angle(-x), velocity);
    bool e = R2.Turn(new Angle(), velocity);
    bool f = R3.Turn(new Angle(x), velocity);
    return a && b && c && d && e && f;
}

//避震检测
void checkBZ(){
    if(!L1.isOnGround()){
        L1.Rise(L1.distanceToGround(),4);
    }
    if(!L2.isOnGround()){
        L2.Rise(L2.distanceToGround(),4);
    }
    if(!L3.isOnGround()){
        L3.Rise(L3.distanceToGround(),4);
    }
    if(!R1.isOnGround()){
        R1.Rise(R1.distanceToGround(),4);
    }
    if(!R2.isOnGround()){
        R2.Rise(R2.distanceToGround(),4);
    }
    if(!R3.isOnGround()){
        R3.Rise(R3.distanceToGround(),4);
    }
}

//车轮类（喊避震系统）
public class Wheel
{
    //方块成员
    public IMyCameraBlock Camera; //摄像头
    public IMyMotorSuspension Motor; //车轮悬架
    public Rotor Rotor; //X转子
    public Piston Piston; //活塞

    //属性
    public int LeftRightRatio = 1; //左右正反转系数，左侧1，右侧-1
    public double TurnAngleLimit = 30; //转向角度限制

    //扫描相关
    public double CameraToWheelDistance = 0.5; //摄像头到轮胎底部距离，可在游戏中启用调试模组测试，它用来判断轮胎是否离地
    public double CameraScanDistance = 5; //摄像头扫描距离

    //车轮相关
    public int WheelLeftRight = 1; //车轮越级正负

    public Wheel(IMyMotorSuspension motor, IMyMotorStator rotor, IMyPistonBase piston, IMyCameraBlock camera){
        this.Motor = motor;

        this.Rotor = new Rotor(rotor);
        this.Rotor.accuracy_ratio = 1*Math.PI/180;
        this.Rotor.accuracy_damping = 6*Math.PI/180;

        this.Piston = new Piston(piston);

        this.Camera = camera;
    }

    //车轮运动
    // input velocity 速度，范围-1到1
    public void Move(double velocity = 0){
        this.Motor.SetValueFloat("Propulsion override", (float)velocity*WheelLeftRight);
    }

    //转向
    public bool Turn(Angle angle = null, double velocity = 0){
        if(angle == null){
            angle = new Angle();
        }
        return this.Rotor.setAngleTo(angle, velocity);
    }

    //锁定、解锁转子
    public void RotorLock(bool isLock = true){
        this.Rotor.Lock(isLock);
    }

    //升高
    public bool Rise(double x = 2, double velocity = 0){
        return this.Piston.setLengthTo(x, velocity);
    }

    //判断是否离地
    public bool isOnGround(){
        this.Camera.EnableRaycast = true; //启用扫描
        MyDetectedEntityInfo target = this.Camera.Raycast(this.CameraToWheelDistance); //扫描50米
        return !target.IsEmpty();
    }

    //获取离地距离
    public double distanceToGround(){
        this.Camera.EnableRaycast = true; //启用扫描
        MyDetectedEntityInfo target = this.Camera.Raycast(this.CameraScanDistance); //扫描50米
        if(!target.IsEmpty()){
            Vector3D hitpoint = new Vector3D();
            Vector3D.TryParse(target.HitPosition.ToString(), out hitpoint);
            return Vector3D.Distance(hitpoint, this.Camera.GetPosition()) - this.CameraToWheelDistance;
        }else{
            return 0;
        }
    }
}

//转子控制类
public class Rotor
{
    public IMyMotorStator R;
    public int PosNeg = 1; //正反转系数
    public double accuracy_ratio = 3*Math.PI/180; //归位判定角度
    public double accuracy_damping = 180*Math.PI/180; //满速转动判定角度
    public Rotor(IMyMotorStator rotor, int posnag = 1){
        this.R = rotor;
        this.PosNeg = posnag;
    }

    //转子控制方法
    //input x目标角度，velocity速度（默认最大），move_type顺时针1或逆时针-1
    public bool setAngleTo(float x, double velocity = 0, int move_type = 0){
		
		//规范输入范围
		if(x > 2*Math.PI) x = (float)(2*Math.PI);
		else if(x < 0) x = 0;

		//计算转动路径路径
		if(move_type == 0){
			if(Math.Abs(x - this.R.Angle) <= 0.5*Math.PI){
				//如果当前与目标点直接弧长小于半周长
				move_type = x - this.R.Angle > 0 ? 1 : -1;
			}else{
				move_type = x - this.R.Angle > 0 ? -1 : 1;
			}
		}

		//计算结果输出量
		float res = 0; //输出范围-1~1
		// if(Math.Abs(x - this.R.Angle) > this.accuracy_damping){
		// 	res = move_type;
		// }else{
		// 	res = Math.Abs(x - this.R.Angle)*move_type/(2*(float)Math.PI);
		// }
        res = Math.Abs(x - this.R.Angle)*move_type/(2*(float)Math.PI);

		res = res*this.PosNeg; //考虑正反系数

		this.R.TargetVelocityRPM = res*30;
		
		return Math.Abs(this.R.Angle - x) < this.accuracy_ratio;
	}
    public bool setAngleTo(double x, double velocity = 0, int move_type = 0){
        return this.setAngleTo((float)x, velocity, move_type);
    }
    public bool setAngleTo(Angle angle, double velocity = 0, int move_type = 0){
        return this.setAngleTo((float)angle.rad, velocity, move_type);
    }

    //锁定和解锁
    public void Lock(bool is_lock = true){
        this.R.RotorLock = is_lock;
    }
}

//活塞控制类
public class Piston
{
    public IMyPistonBase P;
    public double accuracy_ratio = 0.01; //归位判定范围
    public double accuracy_damping = 0.01; //运动归位满速判定范围
    public Piston(IMyPistonBase p){
        this.P = p;
    }

    public bool setLengthTo(double x, double velocity = 0){
        //规范化输入值
        if(x > 2){
            x = 2;
        }else if(x < 0){
            x = 0;
        }

        double res = 0; //输出结果范围，-1到1，活塞的速度范围是-5到5
        if(Math.Abs(x - this.P.CurrentPosition) > accuracy_damping){
            //在归位速度判定范围外，满速归位
            res = x > this.P.CurrentPosition ? 1 : -1;
        }else{
            res = Math.Abs(x - this.P.CurrentPosition)/5;
        }

        //处理速度设置
        if(velocity == 0){
            //不传入则满速
            res*=5;
        }else{
            res*=velocity;
        }

        this.P.Velocity = (float)res;

        return Math.Abs(x - this.P.CurrentPosition) <= this.accuracy_ratio;
    }
}

// ==== 角度运算类 ====
public class Angle
{
	public double angle; //角度
	public double rad{
		get{
			return Angle.getRad(this.angle);
		}
	} //弧度

	public Angle(double value = 0, bool is_rad = false){
		value = is_rad ? Angle.getAngle(value) : value;
		if(Math.Abs(value) > 360){
			value = value%360;
		}
		if(value < 0){
			value = 360 + value;
		}
		this.angle = is_rad ? Angle.getAngle(value) : value;
	}

	//定义加法运算
	public Angle Add(double value, bool is_rad = false){
		value = is_rad ? Angle.getAngle(value) : value;
		this.angle = (this.angle + value) % 360 > 0 ? (this.angle + value) % 360 : 360 + ((this.angle + value) % 360);
		return this;
	}
	public static Angle operator+(Angle ag_1, Angle ag_2){
		return new Angle().Add(ag_1.angle).Add(ag_2.angle);
	}

	// 根据角度获取弧度
	public static double getRad(double angle){
		return angle*Math.PI/180;
	}
	// 根据弧度获取角度
	public static double getAngle(double rad){
		return rad*180/Math.PI;
	}
}

//从分组中获取方块的辅助方法
IMyTerminalBlock getGroupBlocksByBlockName(string group_name, string block_name){
	List<IMyBlockGroup> groups = new List<IMyBlockGroup>();
	GridTerminalSystem.GetBlockGroups(groups);
	foreach(IMyBlockGroup g in groups){
		if(g.Name == group_name){
			List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
			g.GetBlocks(blocks);
			foreach(IMyTerminalBlock b in blocks){
				if(b.CustomName == block_name){
					return b;
				}
			}
		}
	}
	return null;
}

// ==== 调起保存后自动运行 ====
Program(){
	Runtime.UpdateFrequency = UpdateFrequency.Update1;
}